// drops.cpp: rehabilitation catching drops game 
//---------------------------------------------------------------------------
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fstream>
#include <time.h>

//---------------------------------------------------------------------------
#include "chai3d.h"
//---------------------------------------------------------------------------
#include "quanser_signal.h"

//---------------------------------------------------------------------------
// DECLARED CONSTANTS
//---------------------------------------------------------------------------

const double front  =  0.26;
const double back   =  0.26;
const double bottom = -0.26;

// initial size (width/height) in pixels of the display window
const int WINDOW_SIZE_W = 510;
const int WINDOW_SIZE_H = 450;

// mouse menu options (right button)
const int OPTION_FULLSCREEN     = 1;
const int OPTION_WINDOWDISPLAY  = 2;

// esta dato deberia leerlo del CMyCustomDevice
double workspaceScaleFactor = 2.0;

double stiffnessMax;

//---------------------------------------------------------------------------
// DECLARED VARIABLES
//---------------------------------------------------------------------------

// a world that contains all objects of the virtual environment
cWorld* world;

// a camera that renders the world in a window display
cCamera* camera;

// a light source to illuminate the objects in the virtual scene
cLight *light;

// a little "chai3d" bitmap logo at the bottom of the screen
cBitmap* logo;

// background image for game
cBitmap* backgnd;
cGenericObject* bg;

// width and height of the current window display
int displayW  = 0;
int displayH  = 0;

// a haptic device handler
cHapticDeviceHandler* handler;

// a haptic device information
cHapticDeviceInfo info;

// vector for storing haptic device position
cVector3d newPosition;

// vector for locating springs
const cVector3d spring1(-0.15,0.0,0.0);
const cVector3d spring2( 0.15,0.0,0.0);

// haptic device object
cGenericHapticDevice* hapticDevice;

// a table containing pointers to label which display the position of
// each haptic device
cLabel* statsLabel;
cLabel* titleLabel;
cLabel* fpsLabel;
cLabel* timeLabel;
cLabel* positionLabel;
cLabel* forceLabel;
cGenericObject* rootLabels;

// number of haptic devices detected
int numHapticDevices = 0;

// table containing a list of 3D cursors for each haptic device
cShapeTorus* newCursor;
const double innerRad = 0.007;
const double outerRad = 0.03;


// table containing a list of lines to display velocity
cShapeLine* velocityVector;
cShapeLine* back1;
cShapeLine* back2;
cShapeLine* back3;
cShapeLine* back4;
cShapeLine* back5;

// Drop
cShapeSphere* drop;
cTexture2D* dropTexture;
cVector3d dropCenter;
const double dropRadius = 0.01;
const double dropHeight = 0.18;
double speed = 0.0001;
double speedIncrement = 0.00005;
bool isDropCreated = false;
double randDropX, m_x, m_y, m_z;
double heightForCatch = 0.005;
double dropLowestHeight = -0.025;
bool wireMode = false;

// variables for scoring
const double minScoreRadiusX = 0.012;
const double minScoreRadiusY = 0.012;
bool userScored;
int score = 0;
int globalScore = 0;
int failed = 0;
int level = 1;

// material properties used to render the color of the cursors
cMaterial matCursorButtonON;
cMaterial matCursorButtonOFF;

// status of the main simulation haptics loop
bool simulationRunning = false;

// root resource path
string resourceRoot;

// damping mode ON/OFF
bool useDamping = false;

// force field mode ON/OFF
bool useSpring = false;

// spring to drop
bool useSpringToDrop = false;

// has exited haptics simulation thread
bool simulationFinished = false;

// Log file
std::fstream logFile;
bool finishedLogging = false;

// Time variables
ULONGLONG timeStart		  = 0;		// Start of simulation
ULONGLONG timeHaptics	  = 0;		// Ellapsed time (haptics loop updated)
//clock_t	  timeHaptics	  = 0;		// Ellapsed time (haptics loop updated)
ULONGLONG timeHapticsPrev = 0;		// Previus ellapsed time (haptics loop updated)
//clock_t   timeHapticsPrev = 0;		// Previus ellapsed time (haptics loop updated)
ULONGLONG timeGraphics	  = 0;		// Ellapsed time (graphics loop updated)
ULONGLONG timePrevFrame	  = 0;		// Time of previus frame
ULONGLONG timeElapsed	  = 0;
double fx = 0.0;

// Current frame
int	   frame = 0;
double fps = 0.0;

//---------------------------------------------------------------------------
// DECLARED MACROS
//---------------------------------------------------------------------------
// convert to resource path
#define RESOURCE_PATH(p)    (char*)((resourceRoot+string(p)).c_str())


//---------------------------------------------------------------------------
// DECLARED FUNCTIONS
//---------------------------------------------------------------------------

// callback when the window display is resized
void resizeWindow(int w, int h);

// callback when a keyboard key is pressed
void keySelect(unsigned char key, int x, int y);

// callback when the right mouse button is pressed to select a menu item
void menuSelect(int value);

// function called before exiting the application
void close(void);

// function called for drop creation
void createDrop(void);

// main graphics callback
void updateGraphics(void);

// main haptics loop
void updateHaptics(void);

// returns current system time in ms
inline ULONGLONG time_ms();


//===========================================================================
/*
    DEMO:    drops.cpp

    This application is a drop catching game that can measure cathced drops

*/
//===========================================================================

int main(int argc, char* argv[])
{
	//-----------------------------------------------------------------------
	// INITIALIZATION
	//-----------------------------------------------------------------------
	
	printf ("\n");
	printf ("-----------------------------------\n");
	printf ("Biomechatronics Lab using CHAI3D\n");
	printf ("Demo: Drop catching game\n");
	printf ("Copyright 2012\n");
	printf ("-----------------------------------\n");
	printf ("\n\n");
	printf ("Keyboard Options:\n\n");
	printf ("[1] - Render attraction force\n");
	printf ("[2] - Render viscous environment\n");
	printf ("[3] - Render spring to drop\n");
	printf ("[w] - Wire mode\n");
	printf ("[x] - Exit application\n");
	printf ("\n\n");
	
	// parse first arg to try and locate resources
	resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
	
	logFile.open( "data.txt", std::ios_base::out );
	logFile << "time,pos,drop" << std::endl ;
	
	//-----------------------------------------------------------------------
	// 3D - SCENEGRAPH
	//-----------------------------------------------------------------------
	
	// create a new world.
	world = new cWorld();
	
	// set the background color of the environment
	// the color is defined by its (R,G,B) components.
	world->setBackgroundColor(0.0, 0.0, 0.0);
	
	// create a camera and insert it into the virtual world
	camera = new cCamera(world);
	world->addChild(camera);
	
	// position and oriente the camera
	camera->set( cVector3d (0.0, 0.5, 0.1),    // camera position (eye)
	             cVector3d (0.0, 0.0, 0.0),    // lookat position (target)
	             cVector3d (0.0, 0.0, 1.0));   // direction of the "up" vector
	
	
	
	// set the near and far clipping planes of the camera
	// anything in front/behind these clipping planes will not be rendered
	camera->setClippingPlanes(0.01, 20.0);
	
	// create a light source and attach it to the camera
	light = new cLight(world);
	camera->addChild(light);                   // attach light to camera
	light->setEnabled(true);                   // enable light source
	light->setPos(cVector3d( 2.0, 0.5, 1.0));  // position the light source
	light->setDir(cVector3d(-2.0, 0.5, 1.0));  // define the direction of the light beam
	
	
	//-----------------------------------------------------------------------
	// BACKGROUND
	//-----------------------------------------------------------------------
	
	//bg = new cGenericObject();
	//camera->m_back_2Dscene.addChild(bg);
	
	back1 = new cShapeLine(cVector3d(-front,  back,  bottom),cVector3d(-front, -back,  bottom));
	back2 = new cShapeLine(cVector3d(-front, -back,  bottom),cVector3d(-front, -back, -bottom));
	back3 = new cShapeLine(cVector3d(-front, -back,  bottom),cVector3d( front, -back,  bottom));
	back4 = new cShapeLine(cVector3d( front, -back,  bottom),cVector3d( front, -back, -bottom));
	back5 = new cShapeLine(cVector3d( front, -back,  bottom),cVector3d( front,  back,  bottom));
	
	world->addChild(back1);
	world->addChild(back2);
	world->addChild(back3);
	world->addChild(back4);
	world->addChild(back5);
	//create a background with mario bros image
	
	backgnd = new cBitmap();/*
	if(backgnd->m_image.loadFromFile(RESOURCE_PATH("resources\\images\\backchafa.bmp")) == 0)
		printf("Image background loaded\n");
	backgnd->enableTransparency(true);
	camera->m_back_2Dscene.addChild(backgnd);
	
	//bg->setTexture(backgnd);
	//bg->setUseTexture(true);
	
	*/
	//-----------------------------------------------------------------------
	// HAPTIC DEVICES / TOOLS
	//-----------------------------------------------------------------------
	
	// create a haptic device handler
	handler = new cHapticDeviceHandler();
	
	// create a node on which we will attach small labels that display the
	// position of each haptic device
	rootLabels = new cGenericObject();
	camera->m_front_2Dscene.addChild(rootLabels);
	
	// create a small label as title
	titleLabel = new cLabel();
	rootLabels->addChild(titleLabel);
	
	// define its position, color and string message
	titleLabel->setPos(0, 0, 0);
	titleLabel->m_fontColor.set(1.0, 1.0, 1.0);
	titleLabel->m_string = "Game Stats: ";
	
	// get a handle to the next haptic device
	handler->getDevice(hapticDevice, 0);
	
	// open connection to haptic device
	hapticDevice->open();
	
	// initialize haptic device
	hapticDevice->initialize();
	
	// retrieve information about the current haptic device
	info = hapticDevice->getSpecifications();
	
	// set maximum stiffness for haptic effects
	stiffnessMax = info.m_maxForceStiffness / workspaceScaleFactor;
	
	// create a cursor by setting its radius
	newCursor = new cShapeTorus(innerRad,outerRad);
	
	// add cursor to the world
	world->addChild(newCursor);
	
	// create a small line to illustrate velocity
	velocityVector = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));
	
	// add line to the world
	world->addChild(velocityVector);
	
	// if the device provided orientation sensing (stylus), a reference
	// frame is displayed
	if (info.m_sensedRotation == true)
	{
	    // display a reference frame
	    newCursor->setShowFrame(true);
	
	    // set the size of the reference frame
	    newCursor->setFrameSize(0.05, 0.05);
	}
	
	// crate a small label to indicate the position of the device
	statsLabel = new cLabel();
	rootLabels->addChild(statsLabel);
	statsLabel->setPos(0, 20, 0);
	statsLabel->m_fontColor.set(0.6, 0.6, 0.6);
	
	fpsLabel = new cLabel();
	camera->m_front_2Dscene.addChild(fpsLabel);
	fpsLabel->setPos(0, WINDOW_SIZE_H-10, 0);
	fpsLabel->m_fontColor.set(0.8, 0.8, 0.8);
	
	timeLabel = new cLabel();
	camera->m_front_2Dscene.addChild(timeLabel);
	timeLabel->setPos(WINDOW_SIZE_W - 150, WINDOW_SIZE_H-10, 0);
	timeLabel->m_fontColor.set(0.8, 0.8, 0.8);

	positionLabel = new cLabel();
	camera->m_front_2Dscene.addChild(positionLabel);
	positionLabel->setPos(0, WINDOW_SIZE_H-30, 0);
	positionLabel->m_fontColor.set(0.6, 0.6, 0.6);

	forceLabel = new cLabel();
	camera->m_front_2Dscene.addChild(forceLabel);
	forceLabel->setPos(0, WINDOW_SIZE_H-50, 0);
	forceLabel->m_fontColor.set(0.6, 0.6, 0.6);
	
	// here we define the material properties of the cursor when the
	// user button of the device end-effector is engaged (ON) or released (OFF)
	
	// a light orange material color
	matCursorButtonOFF.m_ambient.set(0.5, 0.2, 0.0);
	matCursorButtonOFF.m_diffuse.set(1.0, 0.5, 0.0);
	matCursorButtonOFF.m_specular.set(1.0, 1.0, 1.0);
	
	// a blue material color
	matCursorButtonON.m_ambient.set(0.1, 0.1, 0.4);
	matCursorButtonON.m_diffuse.set(0.3, 0.3, 0.8);
	matCursorButtonON.m_specular.set(1.0, 1.0, 1.0);
	
	newCursor->m_material = matCursorButtonOFF;	
	
	//-----------------------------------------------------------------------
	// OPEN GL - WINDOW DISPLAY
	//-----------------------------------------------------------------------
	
	// initialize GLUT
	glutInit(&argc, argv);
	
	// retrieve the resolution of the computer display and estimate the position
	// of the GLUT window so that it is located at the center of the screen
	int screenW = glutGet(GLUT_SCREEN_WIDTH);
	int screenH = glutGet(GLUT_SCREEN_HEIGHT);
	int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
	int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
	
	// initialize the OpenGL GLUT window
	glutInitWindowPosition(windowPosX, windowPosY);
	glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
	glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
	glutCreateWindow(argv[0]);
	glutDisplayFunc(updateGraphics);
	glutKeyboardFunc(keySelect);
	glutReshapeFunc(resizeWindow);
	glutSetWindowTitle("CHAI 3D");
	
	// create a mouse menu (right button)
	glutCreateMenu(menuSelect);
	glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
	glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
	glutAttachMenu(GLUT_RIGHT_BUTTON);
	
	//-----------------------------------------------------------------------
	// START SIMULATION
	//-----------------------------------------------------------------------
	
	// simulation in now running
	simulationRunning = true;
	
	timeStart = timeHaptics = timeHapticsPrev = timeGraphics = timePrevFrame = time_ms();
	//timeHaptics = timeHapticsPrev = clock();
	
	// create a thread which starts the main haptics rendering loop
	cThread* hapticsThread = new cThread();
	hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
	
	// close everything at exit
	atexit(close);
	
	// start the main graphics rendering loop
	glutMainLoop();
	
	// exit
	return (0);
}

//---------------------------------------------------------------------------

void resizeWindow(int w, int h)
{
    // update the size of the viewport
    displayW = w;
    displayH = h;
    glViewport(0, 0, displayW, displayH);

    // update position of labels
    rootLabels->setPos(displayW-350, displayH-400, 0);
	fpsLabel->setPos(0, displayH-10, 0);
	timeLabel->setPos(displayW - 150, displayH-10, 0);
	positionLabel->setPos(0, displayH-30, 0);
	forceLabel->setPos(0, displayH-50, 0);
	//bg->setPos(5,5,0);
	backgnd->setPos(5,5,0);
}

//---------------------------------------------------------------------------

void keySelect(unsigned char key, int x, int y)
{
    // escape key
    if ((key == 27) || (key == 'x'))
    {
        // exit application
        exit(0);
    }

    // option 1:
    if (key == '1')
    {
        useSpring = !useSpring;
        if (useSpring)
        {
			useSpringToDrop = false;
            printf ("- Springs enabled\n");
        }
        else
        {
            printf ("- Springs disabled\n");
        }
    }

    // option 2:
    if (key == '2')
    {
        useDamping = !useDamping;
        if (useDamping)
        {
            printf ("- Enable viscosity\n");
        }
        else
        {
            printf ("- Disable viscosity\n");
        }
    }

	// option 3:
	if (key == '3')
    {
        useSpringToDrop = !useSpringToDrop;
        if (useSpringToDrop)
        {
			useSpring = false;
            printf ("- Spring to drop enabled\n");
        }
        else
        {
            printf ("- Spring to drop enabled\n");
        }
    }

	if (key == 'w')
	{
	    bool useWireMode = newCursor->getWireMode();
		wireMode = !useWireMode;
        newCursor->setWireMode(!useWireMode);
	}
}

//---------------------------------------------------------------------------

void menuSelect(int value)
{
    switch (value)
    {
        // enable full screen display
        case OPTION_FULLSCREEN:
            glutFullScreen();
            break;

        // reshape window to original size
        case OPTION_WINDOWDISPLAY:
            glutReshapeWindow(WINDOW_SIZE_W, WINDOW_SIZE_H);
            break;
    }
}

//---------------------------------------------------------------------------

void close(void)
{
	// stop the simulation
	simulationRunning = false;
	
	// wait for graphics and haptics loops to terminate
	while (!simulationFinished) { cSleepMs(100); }
	
	hapticDevice->close();

	logFile.close();
}

//---------------------------------------------------------------------------
void createDrop(void)
{
	//cGenericEffect* newEffect;

	// create a drop
	drop = new cShapeSphere(dropRadius);// new drop sphere
	
	// generate pseudo-random position of drop at height = dropHeight
	randDropX = (((rand()%25)/100.0)-0.12);
	dropCenter.set(randDropX, 0, dropHeight);
	drop->setPos(dropCenter);
	dropTexture = new cTexture2D();
	
	if(dropTexture->loadFromFile(RESOURCE_PATH("resources\\images\\droptexture.bmp")) == 0)
		//printf("Texture for drop loaded");
	drop->setTexture(dropTexture);
	drop->setUseTexture(true,true);
	//drop->setShowEnabled(true);
	drop->m_material.setStiffness(1.0 * stiffnessMax);
	//newEffect = new cEffectSurface(drop);
	//drop->addEffect(newEffect);
	
	world->addChild(drop);
	userScored = false;
	drop->setWireMode(wireMode);
	isDropCreated = true;
}
//---------------------------------------------------------------------------

void updateGraphics(void)
{
	frame++;

	timeGraphics = time_ms();

	if( frame == 1 )
		timePrevFrame = timeGraphics ;
	else if ( frame == 30 )
	{
		frame = 0;
		fps = 30000.0 / (double)(timeGraphics-timePrevFrame);
	}
	
	ULONGLONG tmp = timeGraphics - timeStart;
	
	unsigned int minutes = tmp/60000;
	unsigned int milisec = (tmp%60000);
	unsigned int seconds = milisec/1000;
	milisec = milisec % 1000;

	string strLabel;

	cStr(strLabel,fps);

	fpsLabel->m_string = strLabel + " FPS";

	strLabel.clear();

	char buffer[65];
	
	_ui64toa(minutes,buffer,10);
	strLabel.append(buffer);
	strLabel.append(":");

	_ui64toa(seconds,buffer,10);
	if(seconds < 10 )
		strLabel.append("0");
	strLabel.append(buffer);
	strLabel.append(":");

	_ui64toa(milisec,buffer,10);
	if(milisec < 10 )
		strLabel.append("00");
	else if(milisec < 100 )
		strLabel.append("0");
	strLabel.append(buffer);

	timeLabel->m_string = strLabel;


	// read position of device an convert into millimeters
    cVector3d pos;
    hapticDevice->getPosition(pos);
    pos.mul(1000);

	strLabel.assign("Tool  -  x: ");
	cStr(strLabel, pos.x, 2);
	strLabel = strLabel + "   y: ";
	cStr(strLabel, pos.y, 2);
	strLabel = strLabel + "   z: ";
	cStr(strLabel, pos.z, 2);
	
	positionLabel->m_string = strLabel;

	strLabel.assign("Fx: ");
	cStr(strLabel,fx,4);

	forceLabel->m_string = strLabel;

    // create a string that concatenates game stats
	string theStats = "Global Score: ";
	cStr(theStats, globalScore);
	theStats = theStats + " Failed: ";
	cStr(theStats, failed);
	theStats = theStats + " speed ";
	cStr(theStats, speed*1000);
	theStats = theStats + " level ";
	cStr(theStats, level);
	statsLabel->m_string = theStats;
	
	// render world
	camera->renderView(displayW, displayH);
	
	//printf("Global Score: %i, Failed: %i\n",globalScore, failed);
	
	// Swap buffers
	glutSwapBuffers();
	
	// check for any OpenGL errors
	GLenum err;
	err = glGetError();
	if (err != GL_NO_ERROR) printf("Error:  %s\n", gluErrorString(err));
	
	// inform the GLUT window to call updateGraphics again (next frame)
	if (simulationRunning)
	    glutPostRedisplay();
}

//---------------------------------------------------------------------------

void updateHaptics(void)
{
	const unsigned int channels[] = { 0, 1, 2 };
//	double buffer[3];
	string str;
	int j,i;

	unsigned int channel_d = 0;
	char bit = 1;

    // main haptic simulation loop
    while(simulationRunning)
    {
		timeHaptics = time_ms();
		if(timeHapticsPrev>=timeHaptics)
			continue;
        // set drop position
		if (isDropCreated)
		{
			m_x = drop->getPos().x;
			m_y = drop->getPos().y;
			m_z = drop->getPos().z - (speed*(double)(timeHaptics-timeHapticsPrev));
			drop->setPos(m_x, m_y, m_z);

			//Drop below cursor
			if( (m_z <= dropLowestHeight) )
			{	
				// remove drop from world's childs to render
				world->removeChild(drop);

				// eliminate memory allocated
				delete drop;

				// set proper flags
				// userScored = false;
				drop = 0;
				isDropCreated = false;

				if (!userScored)
					failed++;
				newCursor->m_material = matCursorButtonOFF;	
				
				// check if player has failed more than 5 times, if so, game over
				if (failed > 5)
				{
					//failed = 0;
					// Submit score to a file or something
			
					// Display game over graphic

					// end simulation and close devices and stuff
				}
			}
			else
			{
				//if (m_z < dropHeight ) newCursor->m_material = matCursorButtonOFF;
				
				// check if drop's height is near the donut
				if( abs(m_z) < heightForCatch && !userScored)
				{
					// check if player cathced the drop
					if(abs(newPosition.x  - m_x) < minScoreRadiusX )
					{
						userScored = true;
						// do something when scoring
						newCursor->m_material = matCursorButtonON;
						score++;
						globalScore++;
						//cStr(stat, globalScore);
		
						// if player cathces 5 drops increase speed
						if(score > 5)
						{
							score = 0;
							speed += 0.00005;
						}
					}
				}
			}
		}
		else
			createDrop();

		// read position of haptic device
        hapticDevice->getPosition(newPosition);
		
		newPosition.x = newPosition.x * 4.5;
		// la x va de -0.12 a 0.12 aproximadamente
		newPosition.y = newPosition.y * 4.5;

        // read orientation of haptic device
        cMatrix3d newRotation;
        hapticDevice->getRotation(newRotation);

        // update position and orientation of cursor
        newCursor->setPos(newPosition);
        newCursor->setRot(newRotation);

        // read linear velocity from device
        cVector3d linearVelocity;
        hapticDevice->getLinearVelocity(linearVelocity);

        // update arrow
        velocityVector->m_pointA = newPosition;
        velocityVector->m_pointB = cAdd(newPosition, linearVelocity);

        // compute a reaction force
        cVector3d newForce (0,0,0);
		cVector3d tmp(0,0,0);

        // apply spring at two sidess of the catching donut
        if (useSpring)
		{
			double Kp = 100.0; // [N/m]

			newPosition.subr(spring1,tmp);
            cVector3d force = cMul(Kp, tmp);
            newForce.add(force);
			/*
			newPosition.subr(spring2,tmp);
			force = cMul(Kp,tmp);
			newForce.add(force);
			*/
			//printf("newForce.x: %f\n", newForce.x);
        }
		
		if (useSpringToDrop && isDropCreated)
		{
			double Kp = 100.0; // [N/m]
			newPosition.subr(drop->getPos(),tmp);
			cVector3d force = cMul(-Kp, tmp);
			newForce.add(force);
			//printf("newForce.x: %f\n", newForce.x);
        }
        
        // apply viscosity
        if (useDamping)
        {
            cHapticDeviceInfo info = hapticDevice->getSpecifications();
            double Kv = info.m_maxLinearDamping;
            cVector3d force = cMul(-Kv, linearVelocity);
            newForce.add(force);
        }
		if(newForce.x > 50)
			newForce.x = 50;
		else if(newForce.x < -50)
			newForce.x = -50;
		if(newForce.y > 50)
			newForce.y = 50;
		else if(newForce.y < -50)
			newForce.y = -50;

        // send computed force to haptic device
        hapticDevice->setForce(newForce);
		fx = newForce.x;
		
		//if (info.m_modelName == "HARD"/* && (timeHaptics-timeHapticsPrev)>=1*/)
		//{
			//bit ^= 1;
			//((cMyCustomDevice*)hapticDevice)->setDigital(&channel_d,1,&bit);
			//((cMyCustomDevice*)hapticDevice)->getAnalog(channels,ARRAY_LENGTH(channels),buffer);
		timeElapsed = (timeHapticsPrev-timeStart);
		if(timeElapsed<60000)
		{
			j=(timeHaptics-timeHapticsPrev);

			for(i=0; i<j ; i++)
			{
				str.clear();
				cStr(str, (int)(timeHapticsPrev-timeStart)+i+1);
				str.append(",");
				cStr(str, newPosition.x, 4);
				str.append(",");
				cStr(str, m_x, 4);
				logFile << str.c_str() << std::endl ;
			}
		//}
		}
		else if(!finishedLogging)
		{
			finishedLogging = true;
			str.clear();
			str.append("Score: ");
			cStr(str, globalScore);
			str.append("   Failed: ");
			cStr(str, failed);
			logFile << str.c_str() << std::endl ;
		}
		timeHapticsPrev = timeHaptics;
    }
    
    // exit haptics thread
    simulationFinished = true;
}


inline ULONGLONG time_ms()
{
	SYSTEMTIME systemTime;
	GetSystemTime( &systemTime );
	
	FILETIME fileTime;
	SystemTimeToFileTime( &systemTime, &fileTime );
	
	ULARGE_INTEGER uli;
	uli.LowPart = fileTime.dwLowDateTime;
	uli.HighPart = fileTime.dwHighDateTime;
	
	return (uli.QuadPart/10000) ;
}

//---------------------------------------------------------------------------